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ABSTRACT 


The problem of path keeping of marine vehicles along 
Curved paths is considered. In particular, we are concerned 
with circular arcs that join two consecutive straight line 
segments. This provides smooth path changes between one 
segment and the next utilizing a purely geometric reference 
path construction. Pure pursuit guidance is coupled with 
Pwrentation angle control law to ensure stability and 
accuracy. Sensitivity analysis with respect to inaccuracies in 
the knowledge of the vehicle hydrodynamic characteristics is 
performed. The results demonstrate the validity of this 
approach and offer a way to achieve accurate path keeping in 


confined spaces. 
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I. INTRODUCTION 


One of the most important functions of a marine vehicle is 
accurate path keeping through prescribed routes. Path accuracy 
is particularly important when the vehicle has to operate in 
confined waters. In the case of an unmanned vehicle, the 
commanded path is usually approximated by straight line 
segments between consecutive way points. This assumption works 
well if the desired route is a smooth path and can be 
approximated by a series of straight line segments. This has 
been studied extensively in the past. Lienard [1] designed a 
steering sliding mode control autopilot based on the line of 
Sight, while Chism [2] studied a cross track error based 
control law. Hawkinson [3] extended the results to the 
multiple input case when bow and stern planes” are 
independently controlled. Instead of using the cross track 
error directly in the control law, similar path keeping 
characteristics can be achieved by using a line of sight 
pursuit guidance scheme. This is accomplished by moving the 
vehicle in the direction of a moving way point which is 
located on the commanded straight line path [4]. Stability of 
this scheme depends on the control law gains and the look 
ahead distance between the vehicle and the moving way point 


[5]. The technique can be applied to combined steering and 


diving response [6]. In this work, we extend the previous 
studies of pursuit guidance in the case of curved reference 
paths with piecewise constant curvature. This means that we 
adopt a purely geometrical construction of the reference path 
which is composed of a series of straight line segments and 
circular arcs. In Chapter II we present an overview of the 
equations of motion in the horizontal plane, the control and 
the guidance laws. We use the full set of maneuvering 
equations for simulation and a reduced model, Nomoto’s first 
order model, for steering control design. This is because the 
vehicle parameters in the reduced order model can be estimated 
relatively accurately from first principles and experimental 
data [7]. In Chapter III the focus is on stability. We present 
two stability conditions, one based on the reduced model, and 
one based on the complete vehicle dynamics model. The 
sensitivity of the stability curves with respect to the 
vehicle hydrodynamic coefficients and control parameters is 
also studied in this chapter. In Chapter IV we present the 
development and simulation results of the guidance scheme for 
circular reference paths. All computations in this work are 
performed for the NPS AUV II vehicle for which a complete set 
of hydrodynamic coefficients and geometric properties is 
available [8]. Finally, we summarize the conclusions of this 


work and some recommendations for further research. 


II. MATHEMATICAL FORMULATION 


In this section the vehicle equations of motion for the 
horizontal plane (x,y), the control law and the guidance 


scheme are presented. 


A. EQUATIONS OF MOTION 
For the horizontal plane the mathematical model consists 
of the following simplified expressions for the nonlinear Sway 


and yaw differential equations shown below 


m(V+ur+xX,r) =Y (21) 


T,r+mx,(v+ur) =N (2.2) 


Equations (2.1), (2.2) were derived from the general six 
degrees of freedom equations for a vehicle by setting all 
terms related to the vertical plane motion to be zero, 
assuming port/starboard symmetry. The equations for the sway 


force Y and yaw moment N are 
Y=Y r+ (Y,v+Y,ur) +Y uv+Y,u7d 


N=N,1+(N,V+N_ur) +N uv+N,u2d 


Finally the three kinematic equations of the inertial position 


rates and yaw rate are 


W=r (2.3) 
X=ucosw-vsiny (2.4) 
y=usiny+vcosy (2 By 


B. CONTROL LAW 
The coupled equations (2.1) and (2.2) after some algebra 


appear as shown 


V=a,,uv+a,,ur+b,u*d (2.6) 


r=a,,uv+a,,ur+b,u*d (2259) 
where the coefficientS a), A), €@1, €@2, b, and b, are 
D=(T,-N,) (m=Y,,) — (ige-¥.) (ieee) 


sp E(Te-Ny) ¥y= (1%¢6-¥,) Ny] 


a,,> 


[ (I,-N,) (-m+Y,) - (mx,-Y,) (-mx,+N,) ] 


ble 


Q12>= 


(m-Y,) N,- (mx,-N,) Y,] 


al 
421° 


an2== [ (m-Y,) (-mx,+N,) - (mx_-N,) (-m+Y,)] 


b= [ (1,-Nz) Yq, - (mxg-¥4) Ny, - (Ip-Np) Yq, + (mXq-¥,) Np.) 


b, == [ (m-Y,) Ng - (mx,-N,) Y, - (m-Y,) Ng + (mxg-N,) Yo, 1 


Nondimensionilizing the above equations in terms of the water 
density p the vehicle length L and the nominal forward speed 


U of the vehicle, results in the following 


V=a,,V+a,,r+b,8 (2.8) 


r=a,,V+a,,r+b,6 (Z739)) 


From equations (2.8) and (2.9), the relationship of the 
vehicle turning rate r and the rudder angle 6 appears through 


a second order transfer function 


b,S+a,,b,-a,,b, 


i. 
* (Zan 1k0') 


am 3 
S°~ (422 +411) $+4, 1422-41242, 


Performing a Taylor series expansion in the inverse of 
equation (2.10) and keeping the first two terms we arrive at 


a Simplified first order transfer function 


(Zz 11) 


where 


C, =~ (€,,422-42,4,2) (a,,5,-a,,D,) 
Cr = (4,, +422) (a,,D,-a,,D,) +b, (a,,42.-4 2,42) 


C,=- (a,,5,-a,,D,) - 


Equation (2.11) can be also written as 


r=ar+bd 


where 
a=-— and = 


In this way, the vehicle turning dynamics reduce to the 


approximate system of the following two equations 


wer (2 aan 


r=art+bd (2. Ty 


The control law then will be based on yw and r and this is 


going to be in the form 


d=k,y+k,r (2. 7} 


Solving for w from equations (2.12), (2.13) and (2.14) we end 


up with a second order differential equation as below 


Y - (a+bk,) y-bk,p=0 


In terms of the desired natural frequency @, and tne damping 


ratio € the gains k, and k, are given by the following 





relations 
@, 2.15 
k,=- ig (27k) 
a+2C0w 
1S: Se (2.16) 


Therefore, the feedback gains for the control law can be 


determined based on the desired values of @, and ¢. 


C. GUIDANCE 

The heading autopilot that was designed through the 
control law in the previous section is called upon now to 
provide vehicle path keeping through a series of way points in 
the horizontal plane and as we will see later accurate path 
keeping along circular paths. In order to achieve it and at 
the same time to keep the same heading autopilot design we 
have to use a Suitable navigation scheme such as the line of 
Sight guidance scheme (or the look ahead distance scheme). In 


the above scheme the autopilot attempts to position the 


longitudinal axis of the vehicle towards a point D which is 
located ahead of the vehicle’s nominal path at a fixed 

distance das shown in Figure 1. This fixed distance d is the 
look ahead distance and the line of sight angle 6 is defined 


as 


tano=-=— (2 1 


QI 


For pure pursuit navigation the corresponding control law by 


modifying equation (2.14) becomes 


8 =k, (W-Weom) +Kyr (2a 


where 


Wigen=9 


By introducing equation (2.17) in the control law equation, 
the commanded vehicle heading angle is not constant any more 
but a function of the vehicle’s position y as it can be seen 
from equation (2.5). Since the control law was based on 
equations (2.3), (2.6) and (2.7) without including equariena 
(2.5) for lateral displacement, the selected gains k,, k, do 
not guarantee stability any more and therefore conditions must 
be developed to guarantee stability and ensure satisfactory 


path keeping. 


° 
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Figure 1. Geometry and Definitions of Symbols 
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III. STABILITY 


A. APPROXIMATE CONDITION FOR STABILITY 

So far the complete system is given by the approximate 
equations (2.12), (2.13), and the inertial positions 
equation (2.5). In its linearized and dimensionless form for 


a very small lateral velocity v this becomes 


y= (Ss) 


The control law including the guidance scheme is 


$=k, (p+tan 1 5 ) Ree 


which in its linearized form is given by 


b=k, (Wt) +kr (322) 


Solving equations (2.12), (2.13), (3.1) and (3.2) for y welteme 


up to with a third order differential equation as follows : 


k 
y- (a+bk,) ¥-bky- = =0 (3.3) 





10 


Writing the above equation in the s-domain and substituting k, 
and k, from equations (2.15) and (2.16), equation (3.3) takes 


the following form 


2 


5°+2(@,57+w,5+—2 =0 (3.4) 


mae Routh criterion for stability, BC-AD > 0, where A, B, C 
and D are the coefficients of equation (3.4), applied to the 
above equation gives the following condition that must be 


satisfied 





(375) 


If we have selected a natural frequency @, and a damping ratio 
C for the controller, it is possible from the above relation 
to have a quick estimate for the minimum look ahead distance 
d such that the system will be stable. In Figure 2 the 
approximate stability curve (dotted line) was plotted, the 
look ahead distance d versus the natural frequency of the 
controller @,, based on equation (3.5) and for damping ratio 
C=0.8. This is in general considered to be good for a system, 
neither too sluggish, nor too stiff. As is discussed in the 
next section these values for the natural frequency and the 
damping ratio give also reasonable values for the gains k, and 


oe 


ge 





Figure 2. Exact and Approximate Stability Curves for 


Various @,. 


1 


B. EXACT CONDITION FOR STABILITY 
For an exact calculation we use equations (2.3), (2.8), 


(2.9) and equation (2.5) which in linearized form appears as: 


Y=Vt+v ( 3556) 


This set completes the control law while the guidance scheme 
is given equation (3.2). Substituting the control law into the 
equations (2.8), (2.9) and rearranging the terms the system 


becomes as shown 





W=r (3 ° =) ) 
. helo 
vek,_b, yt+a,,v+ (a,,+k,b,) it re (3.8) 
k,b, 
r=k,b,y+a,,v+ (a,.+k,b,) r+ ay (3.9) 
Y=Yrv (S10) 


Local stability properties are established by the eigenvalues 
of the stiffness matrix. The characteristic equation of this 


system is a fourth order polynomial in the form 


AA*+BA?+CA2+DA+E=0 (3as1.) 


where 


ile 


C=C, +C, 


D=D,+D, 


with A, B, C,, C,, Di, D, and E defined as in the following 


B=- (a,,+4,,) -K,b, 


C= (4, 1422-4,242,) + (4), 5, -€,,D,) K,-k,b, 





D, = (a,,.5,-a,,5,) k, 


Ba eerie er 2S 7) 


02 d d 


__(4,,b,-42,D,) k, 


a d 


Routh’ Saecriaceraen for stability determines that loss of 


Stability occurs when the following relation is satisfied 


BCD-B*E-AD*=0 


After some algebra a quadratic equation for d occurs as below: 


a,d*+a,d+a,=0 (3.12) 


14 


where the coefficients Q@,, GQ, G@, are defined as follows 
a, =BC,D,-D{ 
«,=BC,D,d+BC,dD, -B*Ed-2 .0D,D,d 


@,=BC,dD,d-D;d? 


mies DOSsitive root _of equation (3.12).determines the critical 
value of the look ahead distance d for stability. For every d 
> Goriticg CHE System is stable and the vehicle will follow the 
desired path. In the other case when d < doritic4, the system 
becomes unstable and the vehicle moves with an oscillatory 
motion as a result of a complex conjugate pair of eigenvalues 
with positive real parts that appears in this case. In Figure 
2 the continuous line represents the exact stability curve for 
various values of the @, versus the look ahead distance d and 
for a damping coefficient (=0.8. As can be seen the 
approximate stability curve (dotted line) is very close to the 
exact and therefore it can be used as a good first 
approximation in order to provide adequate stability values to 
the system. In Figure 3 a family of curves was plotted for 
damping ratios with values of 1.2 to 0.4 with step 0.1 (left 
to right) for various values of @, versus the look ahead 
distance d. As can be seen the stability area is increasing 
for higher values of the natural frequency @, and for higher 


values of the damping ratio €. When the natural frequency 
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Figure 3. Stability Curves for Various Damping Ratios 


16 


though reaches a value of 3.0 or higher, it is observed that 
the stability curves for the various damping ratios are 
approaching each other giving almost the same stability area. 
From Figure 4 it is also observed that for a natural frequency 
of 3.0 the gains k,, k, are about 3.2 and 1.2 respectively 
while higher values of @ give higher gains without much 
increase in the stability area. Therefore the above selection 
of @ and € to be 3.0 and 0.8 respectively, is considered 
good. An other way to look at the gains is that for 1 degree 
of heading error a rudder of 3.2 degrees is required while for 


[eee e, sec turning rate a 1.2 rudder angle is required. 


C. SENSITIVITY OF STABILITY CURVES 

The hydrodynamic coefficients of the vehicle on which all 
the calculations were based have been determined 
experimentally. Therefore it is realistic to assume that a 
degree of error (uncertainty) that enters the calculations and 
may influence the results. In order to see how much this 
uncertainty for each coefficient changes the stability curves, 
a variation of 50% of the nominal value for each hydrodynamic 
coefficient was assumed and plotted versus the normalized 
look ahead distance d, (d,.,/d,.,). The coefficients that found 
to affect stability were: Y,, Y,, N, and N, while the rest of 
the coefficients had no effect. In Figure 5 the variation of 
the coefficient Y, was plotted versus the normalized look 
ahead distance d,, and as it can be observed a variation of a 
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Figure 4. Control Feedback Gains versus , 
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Figure 5. Changes of Critical Look Ahead Distance d for 


50% of the nominal value gives a total change of about 0.40 
for d,. In Figure 6 the variation of Y, versus d, was plotted 
and for a variation 50% of Y, a total change of about 0.37 of 
d, occurred. In Figure 7 the variation for N, gives a tGm@am 
change of 0.14 for d, while in Figure 8 the variation for N, 
gives a total change of 0.102 for d,. As can be seen the 
variation of Y, and Y, gives values for dad, of about four (4) 
times larger than N, and N,, therefore these two coefficients 
affect the look ahead distance the most. For all the above 
plots above the values for natural frequency and damping ratio 
were 3.0 and 0.8 respectively. In an attempt to see how much 
a variation of € and @, affects the previous results for Y, and 
Y, the following Figures 9, 10, 11 and 12 are plotted. It can 
be seen that the maximum deviation in the critical value of d 
for stability is 40% of its nominal value, whiie for most of 


the cases this deviation is well below 20%. 
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IV. SHIP PATH KEEPING 
After satisfying the requirements for stability by 
selecting proper values for the gains k,, k, and the look ahead 
distance d as was discussed in the previous section, Chemeiie 
path keeping problem along straight line segments and circular 


trajectories is examined in this section. 


A. STRAIGHT LINE SEGMENTS PATH KEEPING 

So far the control law with the guidance scheme is given 
by equation (2.18) which in steady state gives a heading angle 
of zero (0). In order for the vehicle to follow a straight 
line path between two given points (x,, y,) and (x, y,) the 
control law must be modified. If a, is the angle between the 
Straight line (P) and the x axis; the distance y that enters 
the control law becomes y’, as can be seen from Figure 13. The 


modified control law is shown below 


$=k,sin(p-o-a)+k,r (4.1) 


where O and y’ are given by 


O=Y .7=-arctan ( a ) 


y=(y-y,) cosa -(x-x,) sine 
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Figure 13. Guidance Scheme for Straight Line Path 


Zo 


At steady state now the heading angle becomes a and the 
vehicle follows the straight line (P). The simulation program 
takes into account all the possible different orientations of 
(P) by modifying the angle @ properly for each case. In Figure 
14 we can see the simulated path of the vehicle (continuous 
line) and the reference path (dotted) for a two way point 
path. The reference line is a quarter of a circle while the 
two points have coordinates (0,0) and (10,10) respectively. In 
Figure 15 the rudder time history for the above two way point 
path keeping for a straight line is presented. The maximum 
value is about -23 degrees, which is a saturation point since 
the rudder is not allowed to exceed this value (exact 0.4 
radian or ene 3 dequceae In the following Figures 16 
through 23 the number of points was increased to 3, 5, 7 and 
13, in an effort to approach the reference path, by dividing 
the reference path in equally spaced arcs and following the 
straight line segments defined by these points. As can be 
seen, by increasing the number of points the vehicle can 
follow the reference path with increased accuracy. The 
drawbacks of simply increasing the number of discretization 
points are two. First, we are placing increased demands on 
storage and memory for the on-board processor. Second, even 
when satisfactory path accuracy is achieved by using a large 
number of way points the rudder activity is very excessive and 


erratic. Therefore if the vehicle must follow a circular arc 
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Figure 14. Simulation for Reference and Vehicle Path 


for 2 Way Points. 
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Figure 15. Rudder Time History for 2 Way Point Path. 
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Figure 16. Simulation for Reference and Vehicle Path for 


3 Way Points. 
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Figure 17. Rudder Time History for 3 Way Point Path. 
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Figure 18. Simulation for Reference and Vehicle Path for 


5 Way Points. 
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Figure 19. Rudder Time History for 5 Way Point Path. 
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Figure 20. Simulation for Reference and Vehicle Path for 


7 Way Points. 
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Figure 21. Rudder Time History for 7 Way Point Path. 
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Figure 22. Simulation for Reference and Vehicle Path for 


13 Way Points. 
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Figure 23. Rudder Time History for 13 Way Point Path. 
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path, doing so by following small straight line segments is 
not the best way since a large number of points is needed and 
the rudder is doing a considerable amount of effort. This 
means that a different way must be developed such that the 
vehicle at any time t is following the circular arc instead of 


a straight line. 


B. CIRCULAR ARC PATH KEEPING 

By demanding the vehicle to follow a certain circular path 
in a certain way, clockwise or counterclockwise, the angle o 
that enters the control law iS going to vary depending on the 
orientation of the vehicle relative to the center of the path. 
It needs to change values from positive to negative in a 
consistent way with the control law requirements, in order for 
the vehicle to negotiate the assigned path. For this reason we 
need, to properly modify the control law for each case of the 
vehicle motion (cw, ccw) and for each quarter of the circle 
separately. Defining by A the distance from point V to point 


O, Figure 24, A is given by the following expression 


1 
A= [ (x-X9) 2+ (Y-¥p) 7] ? aie 


4l 





~K\ O(x0,y0) 


Figure 24. Guidance Scheme for CW Motion. 
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The radius of the circular path R can also be expressed as 


ul 


R= [ (X,-X9)?+(¥4-¥o) 7] ? ee 


From similar triangles the following relations yield the 


coordinate values of point 3 


X3=Xq+ = (X-X%) (4.4) 


Ys=Vor = (¥-Vo) Soli 


Memeetons (4.2), (4.3), (4.4) and (4.5) are valid for CW or 
CCW motion and for any possible orientation of the vehicle. 
The equations that are valid for each separate case are 


presented below. 


1. CLOCKWISE MOTION 
a) For the fourth quarter of the circle and from Figure 24 


the angle @, can be written as 
X,-X. 
@,-arcsin(—_—) (4.6) 
The angle 8, also appears as follows 
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where 6, can be written as 
aed She yare! 
=——Cc—— — — 4.8 
ei 2 2 R) 


For this case d is the length of the arc from point 3 to Mogae 
4, while d;, is the distance between points 3 and 4 and can be 


written as follows 
d.,=2Rsin (— (4.9) 
a ‘SR’ 


The coordinates of point 4 now since d,,, x; and y; are known 


can be evaluated from trigonometry as follows 


X,=X,+d,,sin6, (2. 


¥,=¥,+d,,cos8, (4 1) 


The distance B from point V to point 4 is 
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1 
Beg (y- ya) | * —_ 
Defining the angle y as following 
y=0+6, (Acgal)) 


and performing the law of sines in the triangle (OV4), Y is 


given by 


: qd,A 
= bien —— ae 
yY=arcsin [sin ( ae = ( ) 


Substituting angle y, equation (4.13) gives angle oO. For the 
control law the angle a (the angle that d;, makes with the 


horizontal) is also needed and this is given by 


a=— -6, (4.15) 


Finally the control law takes the following form 


$=k,sin(y-a+o0) +k,r (4.16) 


Taking the sine of the total heading angle accounts for the 


approximations of linearization that were made in selecting 
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the gains k,, k,. The above form of the control law is valid 
for all of the quarters of the clockwise motion, while for the 
counterclockwise motion, angle 6 iS reversing Sign as we will 
see later. The equations (4.7), (4.8), (4.9), (4.12), (4 
and (4.14) are also valid for all the cases. 

b) For the third quarter, the angle 9; is given by the 


following equation 


@,-arcsin(=°—>) 


The coordinates of point 4 are now given by the following 


expressions 


X,=x,-d,,cos8, 


¥4=Y,+d,,sin8, 


Finally, the angle @ that enters the control law appears as 


below 
a=n -0, 


c) For the second quarter, 9; is given by the following 


equation 
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X,-X 
Sallie 
0, =arcsin(— >—) 

while xX,, Y, are given as 


X,=xX,-d,,sin8, 


¥,=¥,-d,,cos8, 
For this case, the angle @ appear as 
a= 7-0, 
d) Similarly, for the case of the first quarter 


@,-arcsin(—2"2) 


X,=xX,+d,,cos8, 
Waa yo d,,51 00. 


a=-6, 


47] 


2. COUNTERCLOCKWISE MOTION 


From Figure 25 it can be seen that the equations 
(4.2), (4.3), (4.4), (4.5), (4.7), (4.8), (4.3), (43a 


(4.13), (4.14) Gontammes  tomiold- 


a) For the fourth quarter, Figure 25, angle @,, x%,, Yaueue 


Q® appear as follows 


Bi) 


@.=arcsin 
3 ( R 


X,=X,-d,,cos02 
¥,=yY,-d3,sin0, 


a=n +0, 


The control law for this and also for the other three cases of 


the counterclockwise motion appears as below 


$=k,sin(wy-a-o) +k,r 


b) For the third quarter the following relations hold 


6,=arcsin ( = 


X,=x,+d,,sin0, 
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V (x,y) 


X3-x4 
a aoe 
61 
y3-y¥4 
d 
4s 34 





Figure 25. Guidance Scheme for CCW Motion. 
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¥,=y,-d,,cos6, 


a== 7 +8, 


c) For the second quarter similarly we have 


@,-arcsin (2s 


X,=x,+d,,cos8, 
¥,=Y¥,+d,,sin0, 


a=0, 
d) Finally for the first quarter the following equations hold: 


X4-Xy) 


6@.=arcsin 
3 —— 


X,=xX,-d,,s1n0, 


¥4=¥3-d,,cosO, 
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C. SIMULATION RESULTS 

By introducing the modifications of the previous chapter 
into the simulation program, the necessity of providing the 
Simulation program with a set data, in order for the vehicle 
to perform the desired commands, occurred. The table below 
Summarizes the required data, for the simulation program, for 


a test case of a figure - eight manoeuvre shown in Figure 28. 


© 
1 
© 
>) 


NO 
>) 
© 


W 
© 


a 
Ca 


} 
a) 


fs 
© 
}-— 
© 
px 
© 
=) 





Dy 


In the first two columns the coordinates x, y of the next way 
point are given. In the third column, the variable TURN 
selects how the vehicle is going to change its direction for 
the next way point in order to have a smooth change in course 
with less overshoot. There are two ways, 1 for circle, 0 for 
distance. When 1 is selected the vehicle must enter a circle 
with a preselected target radius centered at the way point 

before it starts heading for the next one. When 0 is selected 
the vehicle turns when it reaches a preselected target 
distance from the heading point. The fourth column, 
determines this preselected radius or distance that the 
vehicle has to approach the way point before it starts turmamg 
for the next one. The fifth column determines if the desired 
path 1S a circular are (1) or a straight line segment (0). In 
the sixth column, the (ICCW) index determines if the desired 
path is in the counterclockwise direction CCW for 1, or 
clockwise CW for 0. This is the case, of course, when the 
fifth column as 1 (circular path). Finally, the (woogie 
columns give the coordinates of the center of the circular arc 
if this is the case. In an effort to make a comparison with 
the previous case of Figures 22 and 23, the vehicle path was 
Plotted for a two way circular path. From Figure 26 can be 
seen that the reference path was achieved with the use of only 
two points instead of 13. From Figure 27 we can also see that 
the rudder steers the vehicle in a smooth way achieving much 


smaller values than any other case of straight line segments. 
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Figure 26. Actual and Reference Vehicle Path. 
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Figure 27. Rudder Time History for 


Figure 26. 
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the 


14 


Simulation of 


16 


After introducing the previous table of data into the 
Simulation program, a figure eight manoeuvre was performed by 
the vehicle, Figure 28. The rudder time history of the above 
manoeuvre was also plotted, Figure 29, The small values of the 
rudder angle even for a complicated manoeuvre like this prove 
the advantage of the method, over the use of line segments 
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Figure 28. Figure - Eight (8) Manoeuvre. 


56 


degrees 


20 


15 


10 


=19 


Seow 


i H 5 
‘ j 5 
= 1 
; | i 
: : 
. 
: 


: : 
on ¢ Poems 50n0 ROE $9) OND ED afr mt: em atnsemtnnes cnaecen:  amms 1 en at ate a et ee ee A 
: 


3 “ 
i i 
1 H 
H : 
emt eetmn nen aees ser mes seen. paw ces mene menene tgs atm farwamn ove seeanee ap += ae mares +1 0 48 #0 


oa ace 
as — eee ov amnere: fh atoms gre Geof anen+ eee: =: 
4 
: 
3 
H 
: 
1 
Secor one oe vse o | bee - 
i 
t 
eee ee eee eee eee ee seme eee ee 
1 : 
H : 
; 
; : 
2 i 
4 
: 1 
3 
mes w05 26 meme Sscostoesssoe pore Geer agp me + nome sete on 


10 20 


Figure 29. Rudder Time History for the Manoeuvre of Figure 


28. 
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CONCLUSIONS AND RECOMMENDATIONS 

The main conclusions and contributions of this work can be 
Summarized as follows 

1. The use of Nomoto’s model allows for a very efficient 
steering control design with excellent stability properties 
when coupled to a pursuit guidance scheme. 

2. An approximate stability condition was developed which 
is easy to compute and provides a very good estimate of the 
exact stability condition. 

3. Sensitivity analysis of the stability curves revealed 
the most critical hydrodynamic coefficients for stability. 

4. The use of circular reference paths achieves smooth 
path response along curved segments with very reasonable 
rudder ectiviey . 

Finally, some recommendations for future research include 
the analysis of path accuracy and disturbance rejection 
properties of the scheme, as well as the impact of sensor bias 


and noise. 
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APPENDIX A 


PROGRAM NOMO.FOR 


FOTIS A. PAPOULIAS - DIMITRIOS A. SIMAKIS 
NAVAL POSTGRADUATE SCHOOL 

MONTEREY CALIFORNIA 

JUNE 1992 

CRITICAL STABILITY PROGRAM 

REAL K1,K2,K3,L,NR,NV,NDRS,NDRB,1IZ,MASS, 
& NRDOT, NVDOT 

OPEN (10,FILE=’NOMO.RES’ , STATUS=’ NEW’ ) 


WEIGHT=435.0 


IZ =45.0 
L aS 
RHO =1.94 
G = 32 2 
XG =0.0104 


MASS =WEIGHT/G 
MASS =MASS/(0.5*RHO*L**3) 


IZ =1Z/(0.5*RHO*L**5) 

XG =XG/L 

YRDOT =-0.00000 

YVDOT =-0.03430 

YR =+0.00000 

VANS =-0.10700 

YDRS =+0.01241 

YDRB =+0.01241 

NRDOT =-0.00047 

NVDOT =-0.00000 

NR =-0 .00390 

NV =-0.00000 

NDRS =-0.337%*0.01241 

NDRB =+0.283*0.01241 

DH =(IZ-NRDOT) * (MASS-YVDOT) - 
& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 


AA11=((IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12=((IZ-NRDOT) * (-MASS+YR) - 
& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 


ao 


AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA2Z2= ( (MASS-YVDOT) * (-MASS*XG+NR) - 

& (MASS*XG-NVDOT) * (~MASS+YR) ) /DH 
BB11=((IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12=((IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21=( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 


WRITE (*,1006) 
READ (*,*) RATIO 


BB1=BB11+RATIO*BB12 
BB2=BB21+RATIO*BB22 


WRITE (*,1001) 

READ (*,*) ICON 

GO TO (100,200), Teen 
100 WRITE (*,1002) 


READ (*,*) ZMIN, ZMAX, IZETA 
WRITE (*,1003) 
READ (*,*) WN 
INCR=IZETA 
GO. TO" 50 
200 WRITE (*,1004) 
READ (*,*) WNMIN, WNMAX, IWN 
WRITE (*,1005) 
READ (*,*) ZETA 
INCR=IWN 


50 Cl1=(AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 
C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) 
&+BB2* (AA11*AA22-AA21*AA12) 
C3=-"(A@z 1 “BE I-AA TU Bez 2 
A=C1/C2 
B=C3/C2 


DO 1 I=1,INCR 


IF (ICON.EQ.1) ZETA=ZMIN + (ZMAX -ZMIN ) * (I-1) / (INCR-1) 
IF (ICON.EQ.2) WN =WNMIN+ (WNMAX-WNMIN) * (I-1) / (INCR-1) 


IF (ZCON.BQO.1) OUT=Z2m7A 
IF (ICON.EQ.2) OUT=WN 


K1=-WN*WN/B 
K2=- (A+2.0*ZETA*WN) /B 


B1P=- (AA11+AA22) -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 
& *KZSBB2-k 

C2P=-BBI*Kl1 
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DI P= 1*BB2Z—-AAZ1*BBI*K1 
DZe — (BB ~AAZ2-BB2“AA12) *K1-BB2*K1 
E2P= (AA11*BB2-AA21*BB1) *K1 


AQ=B1P*C1P*D1P-D1P*D1P 
BO=B1P*C1P*D2P+B1P*C2P*D1P-B1P*B1P*E2P-2.0*D1P*D2P 
CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4.0*AQ*CQ 

EE Pb. Ute.) GO TO 1 
XD1=(-BQ+SQRT (DET) ) / (2.0*AQ) 

XD2= (-BQ-SQRT (DET) ) / (2.0*AQ) 


COEF 1=BB1*AA22-BB2*AA12-BB2 
COEF 2=BB2*AA11—-BB1*AA21 


VAL1=1.0+COEF1/ (XD1*COEF2) 
VAL2=1.0+COEF1/ (XD2*COEF2) 
ree VEL ee ee 0) JOR. (XDl.LT.O.0)) XD1=0.0 
et eee Ol) On. (XDZ2.LT.0.0)) XD2=0.0 
XD3=1.0/ (2.0*ZETA*WN) 
Wee 102 0i) X11, XD2, XD3, OUT, KT, K2 

1 CONTINUE 


1001 FORMAT (’ ENTER 1 : ZETA VARIATION’,/, 
is / 2 : WN VARIATION’) 
1002 FORMAT (’ ENTER MIN,MAX, AND INCREMENTS OF ZETA’) 
1003 FORMAT (’ ENTER WN’) 
1004 FORMAT (’ ENTER MIN,MAX, AND INCREMENTS OF WN’) 
1005 FORMAT (’ ENTER ZETA’) 
1006 FORMAT (’ ENTER BOW/STERN RUDDER RATIO’) 
2001 FORMAT (6E15.5) 
END 
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APPENDIX B 
PROGRAM NOMOV.FOR 


FOTIS A. PAPOULIAS =eDIMITRI@SeA. Sitlvaads 
NAVAL POSTGRADUATE SCHOOL 

MONTEREY CALIFORNIA 

JUNE 1992 


SENSITIVITY OF HYDRODYNAMIC COEFFICIENTS sPR@erAM 


REAL K1,K2,K3,L,NR,NV,NDRS,NDRB,IZ,MASS, 
& NRDOT, NVDOT 
OPEN (10,FILE=’NOMOV.RES’ , STATUS=’ NEW’ ) 


WEIGHT=435.0 


IZ =45.0 
L =7.3 
RHO =1.94 
G =e 2 
XG =0.0104 


MASS =WEIGHT/G 
MASS =MASS/ (0 . Sane =m= 3) 


TZ =IZ/(0.5*RHO*L**5) 

XG =XG/L 

YRDOT =-0.00000 

YVDOT ==0),03430 

YR =+0.00000 

YV =—0.10700 

YDRS =+0.01241 

YDRB =+0.01241 

NRDOT =-0.00047 

NVDOT =-0.00000 

NR =-0.00390 

NV =-0.00000 

NDRS =-0.337%*0.01241 

NDRB =+0.283*0.01241 

DH =(IZ-NRDOT) * (MASS-YVDOT) - 
& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 


AA11=((IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AAI2=((PZ-NRDOT) * (=HASSti1) — 
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& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
AA21=( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22=((MASS-YVDOT) * (-MASS*XG+NR) - 

& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
BB11=((IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12=((IZ-NRDOT) * YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21=( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22=( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 


RATIO=-1.0 


BB1=BB11+RATIO*BB12 
BB2=BB21+RATIO*BB22 


ZETA=0.8 
WN=3 .0 


Cl= (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 
C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) +BB2 

& * (AA11*AA22-AA21*AA12) 

C3—— (FAZI* BEI -AA11*BB2) **2 

A=C1/C2 

B=C3/C2 


K1=-WN*WN/B 
K2=- (A+2.0*ZETA*WN) /B 


B1P=- (AA11+AA22) -BB2*K2 

C1P=(AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 
& NZ 513 

C2P=-BB1*K1 

D1P=(AA11*BB2-AA21*BB1) *K1 

D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 

BE2P= (AA11*BB2-AA21*BB1) *K1 


AQ=B1P*C1P*D1P-D1P*D1P 
BQ=B1P*C1P*D2P+B1P*C2P*D1P-B1P*B1P*E2P-2.0*D1P*D2P 
CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQO*BO-4 .0*AQ*CO 

i (DetenT 080). GOnTO 1 

XDNOM= (-BQ+SQRT (DET) ) / (2.0*AQ) 


HOLD=YVDOT 
DO 1 I=1,101 


VARY=0 .5+(I-1) /100.0 
YVDOT=0.5*HOLD+HOLD* (I-1) /100.0 


Pa = (eee) * (MASS-YVDOT) - 
& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
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1 
Fo 


1002 
1003 
1004 
1005 
1006 
2001 


AA11=((IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12=((IZ-NRDOT) * (-MASS+YR) - 

& (MASS *XG-YRDOT) * (-MASS*XG+NR) ) /DH 
AA21=((MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22=((MASS-YVDOT) * (-MASS*XG+NR) - 

& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
BB11=((IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12=((IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21=((MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) * YDRS) /DH 
BB22=((MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 
BB1=BB11+RATIO*BB12 
BB2=BB21+RATIO*BB22 


B1P=- (AA11+AA22) -BB2*K2 
C1P= (AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 


& *KZ eee al 


C2P=—-BB1*K1l 

D1P= (AA11*BB2-AA21*BB1) *K1 

D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 
E2P= (AA11*BB2-AA21*BB1) *K1 


AQ=B1P*C1P*D1P-D1P*D1P 
BQ=B1P*C1P*D2P+B1P*C2P*D1P-B1P*B1P*E2P-2.0*D1P*D2P 
CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4.0*AQ*CQ 

TF (DetsuT. 0.0) GO Tom 
XD1=(-BQ+SQRT (DET) ) / (2.0*AQ) 

XD2= (-BQ-SORT (DET) ) / (2.0*AQ) 


COEF 1=BB1 *AA22-BB2 *AA12-BB2 
COEF 2=BB2*AA11-BB1*AA21 


VAL1=1.0+COEF1/ (XD1*COEF2) 
VAL2=1 .0+COEF1/ (XD2*COEF2) 
IF ((VAL1.LT.0.0) .OR. @&Di. Li On0) eo 
IF ((VAL2.LT.0.0) .OR. (XD2.LT. O00) -be—ome 
XD3=1.0/ (2.0*ZETA*WN) 
WRITE (10,2001) XD1/XDNOM, VARY 

CONTINUE 


FORMAT (’ ENTER 1 : ZETA VARIATION’ ,/, 

& ’ 2 : WN VARIATION.) 

FORMAT (’ ENTER MIN,MAX, AND INCREMENTS OF ZETA’ ) 
FORMAT (’ ENTER WN’) 

FORMAT (’ ENTER MIN,MAX, AND INCREMENTS OF WN’) 
FORMAT (’ ENTER ZETA’) 

FORMAT (’ ENTER BOW/STERN RUDDER RATIO’) 

FORMAT (2E15.5) 

END 
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APPENDIX C 


PROGRAM SIMU.FOR 


Pots A. PAPOULITAS — DIMITRIOS A. SIMAKIS 
NAVAL POSTGRADUATE SCHOOL 

MONTEREY CALIFORNIA 

JUNE 1992 


SIMULATION PROGRAM FOR CONTROL AND GUIDANCE 


ALONG STRAIGHT LINES AND 
CIRCULAR TRAJECTORIES 


REAL K1,K2,L,NR,NV,NDRS,NDRB,1Z,MASS, 
& NRDOT, NVDOT 


OPEN (10,FILE=’ SIMU.RES’ , STATUS=’ NEW’ ) 
OPEN (11,FILE=’ SIMU.DAT’ , STATUS=’ OLD’ ) 


GEOMETRIC PROPERTIES 


WEIGHT=435.0 


IZ =45.0 
L =7.3 
RHO =1.94 
G =32.2 
XG =0.0104 


MASS =WEIGHT/G 
MASS =MASS/(0.5*RHO*L**3) 


IZ =I 47 (0.5*RHO*L**5) 
XG =XG/L 
Jez Jt =3.1415926536 


HYDRODYNAMIC COEFFICIENTS 


YRDOT =~-0.00000 
YVDOT =-0.03430 
YR =+0.00000 
YV =-0.10700 
YDRS =+0.01241 
YDRB =+0.01241 
NRDOT =-0.00047 
NVDOT =-0.00000 
NR =-0.00390 
NV =-0.00000 
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Cee 


NDRS =-0.388707 01228 
NDRB =+0.283*0.01241 


DH =(IZ-NRDOT) * (MASS-YVDOT) - 

& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
AA11=((IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12=((IZ-NRDOT) * (-MASS+YR) - 

& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
AA21=((MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22=((MASS-YVDOT) * (-MASS*XG+NR) - 

& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
BB11=((IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12=((IZ-NRDOT) * YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21=((MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22=( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 


RATIO=-1 


BB1=BB11+RATIO*BB12 
BB2=BB21+RATIO*BB22 


ZETA=0.8 
WN=3 .0 
Dis Le 


C1l=(AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 
C2= (AA11+AA2Z2) * (AA21*BB1-AA11*BB2) +BB2 

& * (AA11*AA22-AA21*AA12) 

C3=- (AA21*BB1-AA11*BB2) **2 

A=C1/C2 

B=C3/C2 


K1=-WN*WN/B 
K2=- (A+2.0*ZETA*WN) /B 


STIME=150.0 
DELTAT=0.1 
ITIME=STIME/DELTAT 


PMETIAL CONDITIONS 


Qux«M MKD <0 


heril i uv ln 
oh loooodow 


PRINT*, ‘ENTER NUMBER OF POINTS” 
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CeO GG Ore) Cr) ) ©) OO) GO ee), Gy 0 0 2 


tit SUMUBAEION STARTS PROM THE POINT (0.0,0.0) AND THE NEXT 


EOINT 


ITURN 


ITURN 
TTURN 


DD 


IPATH 


IPATH 
IPATH 


ICCW 


23 


READ *, NUM 


(32, Yer IS) PROVIDEPmesyY THE DATA FILE. 
DEGSRMINES THE WAY THE VEHICLE IS 
BOeTTURN TC PROCEED FOR THE NEXT POINT. 

1 FOR CIRCBS 

O FOR DISTANCE 


DETERMINES THE DISTANCE OF THE VEHICLE 
FROM XZ,Y2 WHEN THE TURN STARTS. 


DETERMINE SS IE THE DESIRE PATH TO X2,Y2 
> Se erecurAxk ONE OR AVSTRAIGHT LINE. 


1 FOR CIRCULAR 
QO FOR STRAIGHT LINE 


0 FOR CW 
LaoR CCW 


iS ae DES PRS), CENTER OF "THE CIRCULAR PATH 
AND X0,Y0 ARE PROVIDED FROM THE DATA FILE 
AND IF NOT THE DATA 
FIEE PROVIDES ZEKO VALUES. 
Ree ( ieee ez, Y 2 PI TURN, DY, IPATH, IC€W, X0, YO 
SIMULATION STARTS 


DO 1 I=1,ITIME 
TIME=I*DELTAT 


X21=X2-X1 

Y21=Y2-yY1 

DE CUR PbO. 0l) AbD. (Y21.GT.0.)) ANA=+0.5*PI 
Tenn eentipemty2i.i7,0.)) ANA=-0.5*PI 
LE 4X2ie Go. 0e) Go LO 93 
ANA=ATAN (ABS (Y21) /ABS (X21) ) 

i] Pe eeeaeeGEO,) .AND® (Y21.GE.0.)) ANA= ANA 
Paw GEO) AND. (Y¥21.LT.0.)) ANA= -ANA 
i Feeexe bot. 0.) SD. @621.GE.0.)) ANA= PI-ANA 
Leet oT  OeeeAND. (Y21.LT.0.)) ANA=-PI+ANA 


CONTINUE 
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BQUATIONS OF MOTION 


PSIDOT=R 
VDOT=AA11*V+AA12*R+BB1*DR 
RDOT=AA21*V+AA22*R+BB2*DR 
YDOT=SIN (PSI) +V*COS (PSI) 
XDOT=COS (PSI) -V*SIN(PSI) 


FIRST ORDER INTEGRATION 


PSI=PSL On Lr ore T 
V=V+DELTAT*VDOT 
R=R+DELTAT*RDOT 
Y=Y+DELTAT*YDOT 
XR=A+DELTAT* XDOT 


IF (BSIsGT.(2 .O*P1)) ses1-esi—2 203 EL 


YPR= (Y=Y1) *COS (Amey) — (X=X1) “SIN TANA) 
PSIC=-ATAN (YPR/DIST) 
IF (IPATH.EQ.0) DR=K1*SIN((PSI-PSPC-ANA) eae 


IF (IPATH.EQ.1) THEN 

DALF1= ( (X-X0) * (X-X0) + (Y-YO) * (Y-YO) ) **0.5 
RADIUS=( (X1-X0) * (X1-X0) + (Y1-YO) * (Y1-YO) ) **0.5 
X3=X0+RADIUS/DALF1* (X-X0) 

Y3=Y O0+BADIUS/ DAE’ * (y— 70) 

THONE=PI/2.-DIST/ (2.*RADIUS) 

WRITE (*,*) X,Y,X37Y3,7 X07 we RED Is 


LE ai LGCw aE O:0) ase 


IF ((X21.GT 70.0) .AND. (Y2199@Te 080) Teh 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 .0*RADIUS*SIN(DIST/ (2802 bab lUsie 
X4=X3+DISTF*SIN (THTWO) 

Y4=Y3+DISTE*COS (THTWO) 

DALF 2= ( (X=X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=0 .5*PI-THive 

END “ie 


IF ((X21.GT.0.0) .AND. (22) SoC eee 
THTHR=ASIN (ABS (YO0-Y3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 .O@RADIUS*SIN(D ion (25042 bi1Us)) 
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X4=X3+DISTF*COS (THTWO) 
Y4=Ys=bis Tr * SIN (TLaTwo) 

BA 2— 4 ne) + (Y—-Y4) * (Y-Y4).). **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=-THTWO 

END IF 


ce oer On) AND. (Y21.LT.0.0)) THEN 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 
DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3-DISTF*SIN (THTWO) 

Y4=Y3-DISTF*COS (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN(DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 

S IGMA=AALFA-THONE 

THTWO=1 .5*PI-THTWO 

END IF 


mer ez LT. 0.0) . AND. (Y217.GT.0.0)) THEN 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 
THTWO=PI-THONE-THTHR 
DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3-DISTF*COS (THTWO) 

Y4=Y3+DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=PI-THTWO 

END IF 


END oir 
TF © (TCCwW 2021). TaEN 


IF ((X21.GT.0.0) .AND. (Y21.GT.0.0)) THEN 

THTHR=ASIN (ABS (YO-Y3) /RADIUS) 

TE hAO=P1=THONE-THTHR 

Pao le=2. 0-RADLUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3+DISTF*COS (THTWO) 

Y4=Y3+DISTEF*SIN (THTWO) 

DALF 2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
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IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=THTWO 

END IF 


IF ((X21.GT.0.0) AND. (Y21 LT 2020) ) Sie 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 .0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3+DISTF*SIN (THTWO) 
Y4=Y3=DIPSTF*COS (THT We) 

DALF 2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 

SI GMA=AALFA-THONE 

THTWO=1.5*PI+THTWO 

BND ILE 


IF ((X21.LT.0.0) .AND.(Y2Z1 . LT SORG)e ee 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 
THTWO=P1I-THONE~THTHR 

DISTP=2 . (*RADMUS*SIN(DIST/ (2.0*RABIUS)) 
X4=X3-DISTF*COS (THTWO) 

Y4=Y3-DISTF*SIN (THTWO) 
DALP2=(((X-X4) * (X=X4)e (Y-Y4) ~(y-Y 4982-075 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=P1I+THTWO 

END Te 


PF ((X21.LT.0. 0) SAND. (Y2i9er 0. 0) Te 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=P 1 -THONE=THrHaR 

DISTF=2 .0”RADIUS*SIDMDIST/ (270~RADIUS) )} 
X4=X3-DISTF*SIN (THTWO) 

Y4=Y 3*DISTPr*COs ( Pawo) 

DALF2= ( (X~X4) * (X=) +00Y— ae) * (Yea) ) ~*0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT.1.0) BALFA=1.0 

IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 
SIGMA=AALFA-THONE 

THTWO=0 .5*PI+THTWO 

END IF 


END i 
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SMO S 1S ONS ie 


teehee Were) DR=K1*SIN (PSI-THIWO+SIGMA) +K2*R 
ip (ieeieae- 1) DR=h)*SON (PSI-THTWO-SIGMA)4K2*R 


BND IF 


IF (DR.GT.0.4) DR=0.4 
DE (BRwebT.-0.4) DR=-0.4 


WRITE(10,2001) TIME,X,Y,PS1I,PSIC,DR, YPR 


i — (x eZ acne x2) + (Y-YZ) * (Y-Y2) 
TURNC=SQRT (TURN) 
BUEML=SORT (TURN-YPR*YPR) 
TE (ITURN’SBO.1) TD=TURNL 
IF (ITURN.EQ.0) TD=TURNC 
IF (TieieT.eDy THEN 

J=J+1 

IF (J.EQ.NUM) GO TO 2002 

IF (J.NE.NUM) THEN 


X1=X2 
Y1=Y2 
READ (11,*) X2,Y2,ITURN, DD, IPATH, ICCW, XO, YO 
Corto fF 
END IF 
END IF 
CONTINUE 
FORMAT (7E15.5) 
STOP 
END 


PROGRAM REFERENCE PATH 


DIMITRIOS SIMAKIS 

NAVAL POSTGRADUATE SCHOOL 
MONTEREY, CALIFORNIA 

JUNE 1992 


OPEN (11,FILE=’ SIMUR.REF’ , STATUS=’ OLD’ ) 
OPEN (10,FILE=’ SIMUR.RES’ , STATUS='NEW’ ) 
X1=0.0 

Y1=0.0 

PI=3.141 

PRINT*,’ENTER NUMBER OF POINTS’ 

READ*, NUM 


DO 1 I=1,NUM+1 


pall 


IF (I.EQ.1) THEN 
XR=X1 
YR=Y1 
WRITE (10,100) XR,YR 
GO Tomi 
END IF 
READ (11,*) X2,Y2,ITURN, DD, IPATH, ICCW, X0, YO 
THETA=0.0 
RADIUS= ( (X1-X0) * (X1-X0) + (Y1-Y0) * (Y1-Y0) ) **0.5 
X21=X2-X1 
Y21=Y2-yY1l 
DO 2 J=1,200 
THETA= THETA+0.01745 
IF (IPATH.EQ.1) THEN 
IF (ICCW.EQ.0) THEN 
IF (X21.GT.0.0.AND.Y21.GT.0.0) THEN 
XR=X0-RADIUS*COS (THETA) 
YR=Y0+RADIUS*SIN (THETA) 
END IF 


IF (X21 s6T.40. 0. .AND 3421. LEON 0) _ DS GEEN 
XR=X0+RADIUS*SIN (THETA) 
YR=Y0+RADIUS*COS (THETA) 

END IF 


IF (X22.LT.0.0 #AND.Y21.GT.0.0) ,geeN 
XR=X0-RADIUS*SIN (THETA) 
YR=YQ-RADIUS*COS (THETA) 

END IF 


IF (XZ1.U9790.0.AND.Y¥2Zi..L7-70.0) “Oey 
XR=X0+RADIUS*COS (THETA) 
YR=YO-RADIUS*SIN (THETA) 

END IF 

END 


PPM (reCSieEO ae rehiEN 
IF (X21.GT.0.0¢0ND.YZ1 7Gi0 30) THE 
XR=X0+RADIUS*SIN (THETA) 
YR=YO-RADIUS*COS (THETA) 
END wis 


IF (X21.6T .0.0 9D .YZ0cT. 020) HEN 
XR=XO-RADIUS*COS (THETA) 
YR=YOQ-RADIUS*SIN (THETA) 

END IF 


IF (X21.LT.0.0. AND: YZi CT 3070) ae 
XR=X0+RADIUS*COS (THETA) 
YR=YO+RADIUS*SIN (THETA) 

END IF 


J 


Peer, OseND.YZ1.LT.0.0) THEN 
XR=XOQ-RADIUS*SIN (THETA) 
YR=YO+RADIUS*COS (THETA) 

END IF 

END IF 
| iC aa) ol 


IF (GGRATH .&O° 0) THEN 
XR=X2 
YR=Y2 
BND ae 


Writeewtv0,100) XR,YR 
Ei eee 2 (XR —-X2Z) + (YR-Y2Z) * (YR-Y2Z) ) **075 
PEe (bomen l 0.1) THEN 
X1=X2 
Y1=Y2 
GO whom) 
END LF 
CONTINUE 
BORMA TE s6(2.51 5.5% 
CONTINUE 
Sy UO 
END 


ns. 
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